#include "gpio.h"
#include "asm/adc_api.h"
#include "asm/mcpwm.h"
#include "servo_dev.h"
// adc采集
#define ADC_CH AD_CH_PA7
#define ADC_GPIO IO_PORTA_07

#define motor_B1 IO_PORTA_00
#define motor_B2 IO_PORTB_07

#define ABS(x) ((x) < 0 ? -(x) : (x))

void user_adc_init()
{
    adc_add_sample_ch(ADC_CH);
    adc_set_sample_freq(ADC_CH, 1);
    gpio_set_direction(ADC_GPIO, 1);
    gpio_set_pull_down(ADC_GPIO, 0);
    gpio_set_pull_up(ADC_GPIO, 0);
}

u16 get_adc()
{
    return adc_get_value(ADC_CH);
}
void print_adc()
{
    printf("adc: %d\n", get_adc());
}
struct pwm_platform_data motor_pwm_p_data;

void motor_mcpwm_init()
{

#if 1
    motor_pwm_p_data.pwm_aligned_mode = pwm_edge_aligned; // 边沿对齐
    motor_pwm_p_data.pwm_ch_num = pwm_ch0;                // 通道号
    motor_pwm_p_data.frequency = 1000;                    // 1KHz
    motor_pwm_p_data.duty = 0;                            // 占空比50%
    motor_pwm_p_data.h_pin = motor_B1;                    // 任意引脚
    motor_pwm_p_data.l_pin = -1;                          // 任意引脚,不需要就填-1
    motor_pwm_p_data.complementary_en = 0;                // 两个引脚的波形, 0: 同步,  1: 互补，互补波形的占空比体现在H引脚上
    mcpwm_init(&motor_pwm_p_data);
#endif

#if 1
    motor_pwm_p_data.pwm_aligned_mode = pwm_edge_aligned; // 边沿对齐
    motor_pwm_p_data.pwm_ch_num = pwm_ch1;                // 通道号
    motor_pwm_p_data.frequency = 1000;                    // 1KHz
    motor_pwm_p_data.duty = 0;                            // 占空比50%
    motor_pwm_p_data.h_pin = motor_B2;                    // 任意引脚
    motor_pwm_p_data.l_pin = -1;                          // 任意引脚,不需要就填-1
    motor_pwm_p_data.complementary_en = 0;                // 两个引脚的波形, 0: 同步,  1: 互补，互补波形的占空比体现在H引脚上
    mcpwm_init(&motor_pwm_p_data);
#endif
    gpio_set_die(motor_B1, 1);
    gpio_set_direction(motor_B1, 0);
    gpio_set_pull_down(motor_B1, 0);
    gpio_set_pull_up(motor_B1, 0);
    gpio_set_output_value(motor_B1, 0);

    gpio_set_die(motor_B2, 1);
    gpio_set_direction(motor_B2, 0);
    gpio_set_pull_down(motor_B2, 0);
    gpio_set_pull_up(motor_B2, 0);
    gpio_set_output_value(motor_B2, 0);
}

void set_motor_pwm(int speed)
{
    if (speed == 0)
    {
        mcpwm_set_duty(pwm_ch0, 0);
        mcpwm_set_duty(pwm_ch1, 0);
    }
    else if (speed > 0)
    {
        // !顺时针
        mcpwm_set_duty(pwm_ch0, 0);
        mcpwm_set_duty(pwm_ch1, speed);
    }
    else if (speed < 0)
    {
        // !逆时针
        mcpwm_set_duty(pwm_ch0, ABS(speed));
        mcpwm_set_duty(pwm_ch1, 0);
    }
}

void run_motor(u16 step, u8 direction)
{
    print_adc();
    while (step--)
    {
        if (direction == 1)
        {
            set_motor_pwm(10000); //! 顺时针
        }
        else
        {
            set_motor_pwm(10000); //! 逆时针
        }
        delay(10000);
        set_motor_pwm(0); //! 逆时针
    }
}

void set_motor_step(u16 angle)
{
    u16 current_angle = get_adc();
    int speed;
    int absolute;
    printf("angle: %d\n", current_angle);
    if (angle - 10 < current_angle && current_angle < angle + 10)
    {
        set_motor_pwm(0);
    }
    else if (current_angle < angle)
    {
        // 顺时针转
        absolute = angle - current_angle;
        run_motor(absolute, 1);
    }
    else if (current_angle > angle)
    {
        // 逆时针转
        absolute = current_angle - angle;
        run_motor(absolute, 0);
    }
}

// 定义PID参数
double Kp = 0.2; // 比例系数
double Ki = 0.5; // 积分系数
double Kd = 0.1; // 微分系数

double target = 0.0;  // 目标值
double current = 0.0; // 当前值

double integral = 0.0;   // 积分项
double prev_error = 0.0; // 上一次误差
#define MAX_OUTPUT 10000.0
#define MIN_OUTPUT -10000.0
int calculatePID(double input)
{
    double error;
    error = target - input; // 计算误差
    // 计算PID控制量
    double proportional = Kp * error;              //?计算比例
    integral += Ki * error;                        //?计算积分
    double derivative = Kd * (error - prev_error); //?计算微分
    // 更新上一次误差
    prev_error = error;

    printf("                           %d   %d   %d\n", (int)error, (int)integral, (int)derivative);
    // 计算PID输出
    double output = proportional + integral + derivative;
    // printf("output %d", (int)output);
    if (output > MAX_OUTPUT)
    {
        output = MAX_OUTPUT;
    }
    else if (output < MIN_OUTPUT)
    {
        output = MIN_OUTPUT;
    }
    // printf("output %d", (int)output);
    return (int)output;
}

int speed;
void set_angle_pid(double dest_angle)
{
    // 更新目标值
    target = (double)dest_angle;
    // 更新当前值
    current = (double)get_adc();
    printf("angle: %d   speed: %d\n", (int)current, (int)speed);
    speed = calculatePID(current);
    if (dest_angle - 10 < current && current < dest_angle + 10)
    {
        set_motor_pwm(0);
    }
    else if (current == 0) // 转到从0开始
    {
        printf("dead zone");
        set_motor_pwm(10000); //! 逆时针
    }
    else if (speed > 0)
    {
        // printf("foreward speed: %d\n", speed);
        set_motor_pwm(speed); //! 顺时针
    }
    else if (speed < 0)
    {
        speed = -speed;
        // printf("reversal speed: %d\n", speed);
        set_motor_pwm(speed); //! 逆时针
    }
}



int speed;
int absolute;
int last_absolute;
int abs_erro;
double p;
int last_angle;
void set_angle(u16 dest_angle)
{
    u16 current_angle = get_adc();
    absolute = 2 * (dest_angle - current_angle);
    speed = (int)map_to_range(ABS(absolute), 0, 1023, 140, 1000);

    if (dest_angle - 10 < current_angle && current_angle < dest_angle + 10)
    {
        set_motor_pwm(0);
    }
    else if (current_angle == 0) // 转到从0开始
    {
        set_motor_pwm(10000); //! 顺时针
    }
    else if (dest_angle > current_angle)
    {
        set_motor_pwm(speed * 10); //! 顺时针
    }
    else if (dest_angle < current_angle)
    {
        speed = -speed;            //! 换向
        set_motor_pwm(speed * 10); //! 逆时针
    }
    printf("angle: %d  absolute %d  speed: %d\n", current_angle, absolute, speed * 10);
}

/// @brief 用于范围映射
/// @param num  映射的数字
/// @param old_range 原始范围
/// @param old_max 原始范围的最大值
/// @param new_range 目标范围
/// @param new_max 目标范围的最大值
/// @return
double map_to_range(double num, double old_min, double old_max, double new_min, double new_max)
{
    if (old_min >= old_max)
    {
        // 处理旧的区间为零的情况
        return new_max;
    }

    if (num < old_min || num > old_max)
    {
        // 处理不在旧区间内的情况
        return new_max;
    }
    double scale = (new_max - new_min) / (old_max - old_min);
    if (num == old_min)
    {
        // 处理边界值的情况
        return new_min;
    }
    if (scale >= new_max - new_min)
    {
        // 处理范围溢出的情况
        return new_max;
    }

    return new_min + scale * (num - old_min);
}

/// @brief 用于区间映射
/// @param num
/// @return
int map_range(int num)
{
    // 0 ~ 52 之间的数字
    int old_range = 0;  // 原始范围
    int old_max = 2000; // 原始最大值

    int new_range = 0;   // 目标范围
    int new_max = 10000; // 目标最大值

    int result = map_to_range(num, old_range, old_max, new_range, new_max);
    return result;
}

void _180_servo_control(int angle)
{

    int old_range = 0; // 原始范围
    int old_max = 180; // 原始最大值

    int new_range = 50; // 目标范围
    int new_max = 250;  // 目标最大值

    int result = map_to_range(angle, old_range, old_max, new_range, new_max);

    set_timer_pwm_duty(JL_TIMER0, map_range(result));
    set_timer_pwm_duty(JL_TIMER3, map_range(result));
    printf("duty = %d", result);
}